# -*- coding: utf-8 -*-
"""
Created on Mon Apr 24 16:28:00 2023

@author: 52224
"""
#=****载入所需库****=#
# import pyvisa as pv
import time
import numpy as np
#=****连接探针台****=#
# Probe_Station_Address='ASRL9::INSTR'
# rm=pv.ResourceManager()
# Probe_Station=rm.open_resource(Probe_Station_Address)
# Probe_Station.timeout=15000
#=****移动Chuck函数（带抬针）****=#
#参数1：Probe_Station，探针台变量
#参数2：X，移动的X方向参数
#参数3：Y，移动的Y方向参数
#参数4：axis，可选‘Relative’相对于当前坐标移动，‘Center’移动到中心坐标(X,Y),'Home'在Home坐标系下移动
#参数5：Speed，移动速度，100代表100%速度
#返回参数：[Xerror,Yerror],Xerror,X方向移动的误差，Yerror，Y方向移动的误差
def MoveChuck(Probe_Station,X,Y,axis='Relative',speed=100):
    try:
        Probe_Station.write('MoveChuckSeparation 100')
        time.sleep(0.5)
        Probe_Station.read_raw()
    except:
        print('Cannot move the chuck to separation position, action abord!')
        return -999
    Probe_Station.write('ReadChuckPosition Y H')
    time.sleep(0.01)
    Z_position=str(Probe_Station.read_raw(),'UTF-8')
    Z_position=np.float32(Z_position[2:-1].split(' '))
    Z_position=Z_position[2]
    if abs(Z_position) < 200:
        print('Cannot move the chuck to separation position, action abord!')
        return -999
    Probe_Station.write('ReadChuckPosition Y C')
    time.sleep(0.01)
    Original_pos=str(Probe_Station.read_raw(),'UTF-8')
    Original_pos=np.float32(Original_pos[2:-1].split(' '))
    if axis == 'Relative':
        cmd='MoveChuck '+str(X)+' '+str(Y)+' R'+' Y '+str(speed)
        Predict_pos = [Original_pos[0]+X]
        Predict_pos = Predict_pos+[Original_pos[1]+Y]
    elif axis == 'Center':
        cmd='MoveChuck '+str(X)+' '+str(Y)+' C'+' Y '+str(speed)
        Predict_pos = [X]
        Predict_pos = Predict_pos+[Y]
    elif axis == 'Home':
        cmd='MoveChuck '+str(X)+' '+str(Y)+' H'+' Y '+str(speed)
        Predict_pos = [X]
        Predict_pos = Predict_pos+[Y]
    Probe_Station.write(cmd)
    time.sleep(0.01)
    Probe_Station.read_raw()
    if axis == 'Home':
        Probe_Station.write('ReadChuckPosition Y H')
    else:
        Probe_Station.write('ReadChuckPosition Y C')
    time.sleep(0.01)
    pos=str(Probe_Station.read_raw(),'UTF-8')
    pos=np.float32(pos[2:-1].split(' '))
    try:
        Probe_Station.write('MoveChuckContact 10')
        time.sleep(0.5)
        Probe_Station.read_raw()
    except:
        print('Cannot move the chuck to Contact position, action abord!')
        return -999
    Probe_Station.write('ReadChuckPosition Y H')
    time.sleep(0.01)
    Z_position=str(Probe_Station.read_raw(),'UTF-8')
    Z_position=np.float32(Z_position[2:-1].split(' '))
    Z_position=Z_position[2]
    if abs(Z_position) > 10:
        print('Cannot move the chuck to Contact position, action abord!')
        return -999
    return pos[0:2]-Predict_pos[0:2]
#=****读取Chuck位置信息****=#
#参数1：Probe_Station: 输入探针台变量
#参数2：参考系，可选相对于中心'Center'或者相对于Home（Z相对于Contact高度）'Home'
#返回值：如果参数2错误，不会返回任何参数。否则，返回一个list，包含三个数，分别代表X Y Z
def ReadChuckPosition(Probe_Station,axis='Center'):
    if axis=='Center':
        Probe_Station.write('ReadChuckPosition Y C')
        time.sleep(0.01)
    elif axis == 'Home':
        Probe_Station.write('ReadChuckPosition Y H')
        time.sleep(0.01)
    else:
        print('Wrong axis parameter!')
        return
    pos=str(Probe_Station.read_raw(),'UTF-8')
    pos=pos[2:-1].split(' ')
    return np.float32(pos)